/*
The MIT License (MIT)

Copyright (c) 2013 Mike Dapiran, Brian May, Richard Pospesel, and Bert Wierenga

Permission is hereby granted, free of charge, to any person obtaining a copy
of this software and associated documentation files (the "Software"), to deal
in the Software without restriction, including without limitation the rights
to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
copies of the Software, and to permit persons to whom the Software 
furnished to do so, subject to the following conditions:

The above copyright notice and this permission notice shall be included in
all copies or substantial portions of the Software.

THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
THE SOFTWARE.
*/
#include "Transform.h"

#include <cmath>

#include "GameObject.h"
#include "Content\Scene.h"

namespace Hogshead
{
	namespace Core
	{
		/** constructors **/

		Transform::Transform()
		{
			_parent = NULL;
			_position = Vector3(0.0f, 0.0f, 0.0f);
			_rotation = Quaternion(1.0f, 0.0f, 0.0f, 0.0f);
			_scale = Vector3(1.0f,1.0f,1.0f);

			_world_matrix = new Matrix4();
			_world_matrix->toIdentity();
			_world_up_to_date = false;
		}

		Transform::~Transform()
		{
			delete _world_matrix;
		}

		Transform::Transform(GameObject* in_parent)
		{
			_parent = in_parent;
			_position = Vector3(0.0f, 0.0f, 0.0f);
			_rotation = Quaternion(1.0f, 0.0f, 0.0f, 0.0f);
			_scale = Vector3(1.0f,1.0f,1.0f);

			_world_matrix = new Matrix4();
			_world_matrix->toIdentity();
			_world_up_to_date = false;
		}

		Transform::Transform(const Transform& other)
		{
			_parent = NULL;

			_position = other._position;
			_rotation = other._rotation;
			_scale = other._scale;

			_world_matrix = new Matrix4();
			_world_matrix->toIdentity();		
			_world_up_to_date = false;
		}

		Transform& Transform::operator=(const Transform& in_other)
		{
			//give us no parent so we cannot be broken
			_parent = NULL;
			
			//copy data
			_scale = in_other._scale;
			_rotation = in_other._rotation;
			_position = in_other._position;
			_world_up_to_date = false;
			return *this;
		}



		Transform& Transform::copy(const Transform& in_other, GameObject* in_to_be_parent)
		{
			*this = in_other;
			_parent = in_to_be_parent;
			invalidateWorld();
			return *this;
		}
		void Transform::invalidateWorld()
		{
			//invalidate ourself
			_world_up_to_date = false;
			if(!_parent)
				error("this Transform has no parent GameObject");

			//invalidate all of our children
			for(int i = 0; i < _parent->gameObjectCount(); i++)
			{
				_parent->getChildGameObject(i)->_transform.invalidateWorld();
			}
		}
		
		void Transform::initialize(GameObject* in_game_object)
		{
			_parent = in_game_object;
			invalidateWorld();
			//use this to update at init time not first frame
			getWorldMatrix();
		}


		void Transform::setPosition(float x, float y, float z)
		{
			 _position.x = x;
			 _position.y = y;
			 _position.z = z;

			 invalidateWorld();
		}

		void Transform::setPosition(const Vector3& in_position)
		{
			_position = in_position;

			invalidateWorld();
		}

		void Transform::setRotation(const Quaternion& rotation)
		{
			_rotation = rotation;

			invalidateWorld();
		}

		void Transform::setOrientation(const Vector3& forward, const Vector3& up)
		{
			_rotation.setFromBasis(forward, up);
			_rotation.normalize();
			invalidateWorld();
		}

		void Transform::setScale(float x, float y, float z)
		{
			_scale.x = x;
			_scale.y = y;
			_scale.z = z;

			invalidateWorld();
		}

		void Transform::translate(float x, float y, float z)
		{
			_position.x += x;
			_position.y += y;
			_position.z += z;

			invalidateWorld();
		}

		void Transform::translate(const Vector3& in_vec)
		{
			_position += in_vec;
			
			invalidateWorld();
		}

		void Transform::rotateX(float theta)
		{
			Quaternion to_apply(Vector3::right, theta);
			_rotation =  to_apply * _rotation;

			invalidateWorld();
		}

		void Transform::rotateY(float theta)
		{
			Quaternion to_apply(Vector3::up, theta);
			_rotation = to_apply * _rotation;

			invalidateWorld();
		}

		void Transform::rotateZ(float theta)
		{
			Quaternion to_apply(Vector3::forward, theta);
			_rotation = to_apply * _rotation;

			invalidateWorld();
		}

		void Transform::rotate(float x_theta, float y_theta, float z_theta)
		{
			Quaternion temp;

			if(x_theta != 0.0f)
			{
				_rotation = temp.setFromAxisAngle(Vector3::right, x_theta).mul( _rotation );
			}
			if(y_theta != 0.0f)
			{
				_rotation = temp.setFromAxisAngle(Vector3::up, y_theta).mul( _rotation );
			}
			if(z_theta != 0.0f)
			{
				_rotation = temp.setFromAxisAngle(Vector3::forward, z_theta).mul( _rotation );
			}
			_rotation.normalize();
			invalidateWorld();
		}

		void Transform::setRotationFromEuler(float x_angle, float y_angle, float z_angle)
		{
			//get identity quaternion
			Quaternion new_rotation;
			Quaternion temp;
			//rotate for each axis
			if(x_angle != 0.0f)
			{
				new_rotation = temp.setFromAxisAngle(Vector3::right, x_angle).mul( new_rotation );
			}
			if(y_angle != 0.0f)
			{
				new_rotation = temp.setFromAxisAngle(Vector3::up, y_angle).mul( new_rotation );
			}
			if(z_angle != 0.0f)
			{
				new_rotation = temp.setFromAxisAngle(Vector3::forward, z_angle).mul( new_rotation );
			}
			new_rotation.normalize();
			_rotation = new_rotation;

			invalidateWorld();
		}

		void Transform::rescale(float x, float y, float z)
		{
			_scale.x *= x;
			_scale.y *= y;
			_scale.z *= z;
			
			invalidateWorld();
		}

		Vector3 Transform::getWorldPosition() const
		{
			return getWorldMatrix().getTranslation();
		}

		const Vector3& Transform::getLocalPosition() const
		{
			return _position;
		}

		const Vector3& Transform::getLocalScale() const
		{
			return _scale;
		}

		const Quaternion& Transform::getRotation() const
		{
			return _rotation;
		}

		Matrix4 Transform::getLocalMatrix() const 
		{
			//get Rotation Matrix
			Matrix4 result(_rotation);
			//set scale
			result.scale(_scale);
			//set translation
			result.setTranslation( _position );
			return result;
		}

		const Matrix4& Transform::getWorldMatrix() const
		{
			//if(this->_parent->_name.equals("Test SpotLight Left"))
			//	error();

			Vector3 forward = _rotation.getForward();

			if(_world_up_to_date)
				return *_world_matrix;
			//not up-to-date so we need to recalculate

			//change this to be a while loop not recursive 
		
			//take a look at this is just seems messed up right now	
			if(!_parent || !_parent->getParentGameObject())	
			{
				_world_up_to_date = true;
				*_world_matrix = getLocalMatrix();
				return *_world_matrix;
			}
			else
			{
				_world_up_to_date = true;
				*_world_matrix = _parent->getParentGameObject()->getTransform().getWorldMatrix() * getLocalMatrix();
				return *_world_matrix;
			}
		}

		Matrix4 Transform::getWorldRotation()
		{
			//get from world
			Matrix4 result(getWorldMatrix());

			//remove translation
			result.setTranslation(Vector3::zero);
			//remove scale
			result.setRight( result.getRight().normalize() );
			result.setUp( result.getUp().normalize() );
			result.setForward( result.getForward().normalize() );

			return result;
		}

		Vector3 Transform::getLocalRotationEuler() const
		{
			return getLocalMatrix().getRotation();
		}

		Vector3 Transform::getForward() const
		{
			return _rotation.getForward();
		}

		Vector3 Transform::getUp() const
		{
			return _rotation.getUp();
		}

		Vector3 Transform::getRight() const
		{
			return _rotation.getRight();
		}

		
	}
}